#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <iostream>

#include <geometry_msgs/Quaternion.h>
#include <tf2/LinearMath/Quaternion.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Twist.h>

using namespace std;

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

void sleep(double second)
{
    ros::Duration(second).sleep();
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "send_goals_node");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    MoveBaseClient ac("move_base", true);
    ac.waitForServer();
    move_base_msgs::MoveBaseGoal goal;
    tf2::Quaternion quaternion;

    /*
    *****************************第一个点,正前方向前，擦拭上盖****************************************************
    */
    double hover_x = 0.40;      // 马桶正前方点x
    double hover_y = 1.43;      // 马桶正前方点y
    quaternion.setRPY(0, 0, 0); // 朝向前方
    goal.target_pose.pose.position.x = hover_x;
    goal.target_pose.pose.position.y = hover_y;
    goal.target_pose.pose.orientation.z = quaternion.z();
    goal.target_pose.pose.orientation.w = quaternion.w();
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    ac.sendGoal(goal);
    ROS_INFO("MoveBase Send Goal !!!");
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("The MoveBase Goal Reached Successfully!!!");
        system("roslaunch clean_toilet_robot arm_hover.launch");
    }
    else
    {
        ROS_WARN("The MoveBase Goal Planning Failed for some reason");
    }

    /*
    *****************************第二个点，左侧向右，按钮*********************************************************************
    */
    double push_btn_x = 0.4;          // 按键点x
    double push_btn_y = 1.63;         // 按键点y
    quaternion.setRPY(0, 0, -1.5707); // 朝向右侧
    goal.target_pose.pose.position.x = push_btn_x;
    goal.target_pose.pose.position.y = push_btn_y;
    goal.target_pose.pose.orientation.z = quaternion.z();
    goal.target_pose.pose.orientation.w = quaternion.w();
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    ac.sendGoal(goal);
    ROS_INFO("MoveBase Send Goal !!!");
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("The MoveBase Goal Reached Successfully!!!");
        // 抓取餐具
        system("roslaunch clean_toilet_robot arm_push_btn.launch");
    }
    else
    {
        ROS_WARN("The MoveBase Goal Planning Failed for some reason");
    }

    /*
    *****************************第三个点，右侧向前，抓马桶刷****************************************************
    */
    double grab_x = 1.72;       // 抓取点x
    double grab_y = 1.43;       // 抓取点y
    quaternion.setRPY(0, 0, 0); // 朝向前方
    goal.target_pose.pose.position.x = grab_x;
    goal.target_pose.pose.position.y = grab_y;
    goal.target_pose.pose.orientation.z = quaternion.z();
    goal.target_pose.pose.orientation.w = quaternion.w();
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    ac.sendGoal(goal);
    ROS_INFO("MoveBase Send Goal !!!");
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        system("roslaunch clean_toilet_robot arm_grab.launch");
    }
    else
    {
        ROS_WARN("The MoveBase Goal Planning Failed for some reason");
    }

    /*
    *****************************第四个点，右侧向左，刷马桶****************************************************
    */
    double flush_x = 1.72;           // 刷马桶点x
    double flush_y = 1.63;           // 刷马桶点y
    quaternion.setRPY(0, 0, 1.5707); // 朝向左侧
    goal.target_pose.pose.position.x = flush_x;
    goal.target_pose.pose.position.y = flush_y;
    goal.target_pose.pose.orientation.z = quaternion.z();
    goal.target_pose.pose.orientation.w = quaternion.w();
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    ac.sendGoal(goal);
    ROS_INFO("MoveBase Send Goal !!!");
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("The MoveBase Goal Reached Successfully!!!");
        system("roslaunch clean_toilet_robot arm_flush.launch");
    }
    else
    {
        ROS_WARN("The MoveBase Goal Planning Failed for some reason");
    }

    /*
    *****************************第五个点，右侧向前，放马桶刷****************************************************
    */
    goal.target_pose.pose.position.x = grab_x; // 放置点x
    goal.target_pose.pose.position.y = grab_y; // 放置点y
    quaternion.setRPY(0, 0, 0);                // 朝向前方
    goal.target_pose.pose.orientation.z = quaternion.z();
    goal.target_pose.pose.orientation.w = quaternion.w();
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    ac.sendGoal(goal);
    ROS_INFO("MoveBase Send Goal !!!");
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        system("roslaunch clean_toilet_robot arm_put.launch");
    }
    else
    {
        ROS_WARN("The MoveBase Goal Planning Failed for some reason");
    }

    /*
    *****************************第六个点，左侧向右，按开关****************************************************
    */
    quaternion.setRPY(0, 0, -1.5707);              // 朝向右侧
    goal.target_pose.pose.position.x = push_btn_x; // 按开关x
    goal.target_pose.pose.position.y = push_btn_y; // 按开关y
    goal.target_pose.pose.orientation.z = quaternion.z();
    goal.target_pose.pose.orientation.w = quaternion.w();
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    ac.sendGoal(goal);
    ROS_INFO("MoveBase Send Goal !!!");
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("The MoveBase Goal Reached Successfully!!!");
        // 抓取餐具
        system("roslaunch clean_toilet_robot arm_push_btn.launch");
    }
    else
    {
        ROS_WARN("The MoveBase Goal Planning Failed for some reason");
    }

    // 发送返回导航点
    goal.target_pose.pose.position.x = 0.0;
    goal.target_pose.pose.position.y = 0.0;
    goal.target_pose.pose.orientation.z = 0.0;
    goal.target_pose.pose.orientation.w = 1.0;
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    ac.sendGoal(goal);
    ROS_INFO("Send Goal Home !!!");
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("Back !!!!");
    }
    else
    {
        ROS_WARN("The Goal Planning Failed for some reason");
    }

    return 0;
}
